#include "ProtocolHandler.h"
#include "stdlib.h"

/*
	ProtocolHandler library is used to manage protocol communication
	between Arduino smart sensor and PC

	Authors: 	Michael Schöllhammer
				Vladimir Marinkovic

*/
				
String cmd_REQTEMP = "REQTEMP";
String cmd_RESPTEMP = "RESPTEMP";

String cmd_REQX = "REQX";
String cmd_REQY = "REQY";
String cmd_REQZ = "REQZ";
String cmd_RESPX = "RESPX";
String cmd_RESPY = "RESPY";
String cmd_RESPZ = "RESPZ";

String cmd_REQPARA = "REQPARA";
String cmd_RESPPARA = "RESPPARA";

String cmd_SETPARA = "SETPARA";
String cmd_ALARMTEMP = "ALARMTEMP";
String cmd_ALARMX = "ALARMX";
String cmd_ALARMY = "ALARMY";
String cmd_ALARMZ = "ALARMZ";

String cmd_RESETMINMAX = "RESETMINMAX";
String cmd_ACK = "ACK";
String cmd_NACK = "NACK";

//*******************
// INPUTS
//*******************

int tempInput= A0;
int accX = A3;
int accY = A4;
int accZ = A5;
const float arduino_power_supply = 5;
const float sensor_power_supply = 3.3;
const float zero_g_bias = sensor_power_supply / 2;
const float rad2deg = 57.2957795;

//******************
//MIN & MAX
//******************

float minT = -10.00;
float maxT = 50.00;
double accelXMin = -90.00;
double accelXMax = 90.00;
double accelYMin = -90.00;
double accelYMax = 90.00;
double accelZMin = -90.00;
double accelZMax = 90.00;
double dToverdtMax = 0.3;

float minT_b4_reset = 0;
float maxT_b4_reset = 0;
float accelXMin_b4_reset = 0;
float accelXMax_b4_reset = 0;
float accelYMin_b4_reset = 0;
float accelYMax_b4_reset = 0;
float accelZMin_b4_reset = 0;
float accelZMax_b4_reset = 0;
float dToverdtMax_b4_reset = 0;

int autoT_Trans_Sec = 0;
int autoT_Degrees_Changed = 0;


ProtocolHandler::ProtocolHandler(){
  Serial.begin(9600); 
}

//ProtocolHandler::~ProtocolHandler(){}

void ProtocolHandler::sendAck(){}

void ProtocolHandler::sendNack(){}

boolean ProtocolHandler::handleRequest(String inputString)
{
	if(inputString.indexOf(cmd_REQTEMP) != -1){
		double temp = getTempWTable(0, 10000);
		String response = cmd_RESPTEMP+":"+ConvertFloatToString(temp,6,3,2)+"\n";
		sendResponse(response);
		return true;
	}else if(inputString.indexOf(cmd_REQX) != -1){
		float x = voltage2g(analogRead(accX));
		String response; 
		response = cmd_RESPX+":"+ConvertFloatToString(x,9,3,5)+"\n";
		sendResponse(response);
		return true;
	}else if(inputString.indexOf(cmd_REQY) != -1){
		float y = voltage2g(analogRead(accY));
		String response = cmd_RESPY+":"+ConvertFloatToString(y,9,3,5)+"\n";
		sendResponse(response);
		return true;
	}else if(inputString.indexOf(cmd_REQZ) != -1){
		float z = voltage2g(analogRead(accZ));
		String response = cmd_RESPZ+":"+ConvertFloatToString(z,9,3,5)+"\n";
		sendResponse(response);
		return true;
	}else if(inputString.indexOf(cmd_REQPARA) != -1){
		String substr = inputString.substring(8);
		//Serial.println("Substring:"+substr);
		if(substr.equals("1\n")){
			float val = getMaxT(false);
			String response = cmd_RESPPARA+":1,"+ConvertFloatToString(val,6,3,2)+"\n";
			sendResponse(response);
			return true;
		}else if(substr.equals("2\n")){
			float val = getMinT(false);
			String response = cmd_RESPPARA+":2,"+ConvertFloatToString(val,6,3,2)+"\n";
			sendResponse(response);
			return true;
		}else if(substr.equals("3\n")){
			float val = getDToverdfMax(false);
			String response = cmd_RESPPARA+":3,"+ConvertFloatToString(val,6,3,2)+"\n";
			sendResponse(response);
			return true;
		}else if(substr.equals("11\n")){
			double val = getAccelXMax(false);
			char str[6] ;
			dtostrf(val, 2, 2, str);
			String response = cmd_RESPPARA+":11,"+String(str)+"\n";
			sendResponse(response);
			return true;
		}else if(substr.equals("20\n")){
			String response = cmd_RESPPARA+":20,"+ConvertFloatToString(maxT_b4_reset,6,3,2)+"\n";
			sendResponse(response);
			return true;
		}else if(substr.equals("21\n")){
			String response = cmd_RESPPARA+":21,"+ConvertFloatToString(minT_b4_reset,6,3,2)+"\n";
			sendResponse(response);
			return true;
		}else if(substr.equals("30\n")){
			String response = cmd_RESPPARA+":30,"+ConvertFloatToString(accelXMax_b4_reset,6,3,2)+"\n";
			sendResponse(response);
			return true;
		}else if(substr.equals("31\n")){
			String response = cmd_RESPPARA+":31,"+ConvertFloatToString(accelYMax_b4_reset,6,3,2)+"\n";
			sendResponse(response);
			return true;
		}/*else if(substr.equals("32\n")){
			String response = cmd_RESPPARA+":32,"+ConvertFloatToString(accelZMax_b4_reset,6,3,2)+"\n";
			sendResponse(response);
			return true;
		}else if(substr.equals("50\n")){
			String response = cmd_RESPPARA+":50,"+ConvertFloatToString(autoT_Trans_Sec,6,3,2)+"\n";
			sendResponse(response);
			return true;
		}else if(substr.equals("51\n")){
			String response = cmd_RESPPARA+":51,"+ConvertFloatToString(autoT_Degrees_Changed,6,3,2)+"\n";
			sendResponse(response);
			return true;
		}else if(substr.equals("60\n")){
			//String response = cmd_RESPPARA+":60,"+CURRENT_TEMP_FOR_CALIBRATION+"\n";
			//sendResponse(response);
			return true;
		}else if(substr.equals("61\n")){
			//String response = cmd_RESPPARA+":61,"+CURRENT_ACC_X_FOR_CALIBRATION+"\n";
			//sendResponse(response);
			return true;
		}else if(substr.equals("62\n")){
			//String response = cmd_RESPPARA+":62,"+CURRENT_ACC_Y_FOR_CALIBRATION+"\n";
			//sendResponse(response);
			return true;
		}else if(substr.equals("63\n")){
			//String response = cmd_RESPPARA+":63,"+CURRENT_ACC_Z_FOR_CALIBRATION+"\n";
			//sendResponse(response);
			return true;
		}*/else{
			return false;
		}
	}else if(inputString.indexOf(cmd_SETPARA) != -1){
		String substr = inputString.substring(8);
		
		if(substr.indexOf("30,") != -1){
			String valString = inputString.substring(11);
			float v = convertStrToFloat(valString);
			accelXMax_b4_reset = v;
			return true;
		}else if(substr.indexOf("31,") != -1){
			String valString = inputString.substring(11);
			float v = convertStrToFloat(valString);
			accelYMax_b4_reset = v;
			return true;
		}else if(substr.indexOf("32,") != -1){
			String valString = inputString.substring(11);
			float v = convertStrToFloat(valString);
			accelZMax_b4_reset = v;
			return true;
		}else if(substr.indexOf("20,") != -1){
			String valString = inputString.substring(11);
			float v = convertStrToFloat(valString);
			maxT_b4_reset = v;
			return true;
		}else if(substr.indexOf("21,") != -1){
			String valString = inputString.substring(11);
			float v = convertStrToFloat(valString);
			minT_b4_reset = v;
			return true;
		}else if(substr.indexOf("11,") != -1){
			String valString = inputString.substring(11);
			float v = convertStrToFloat(valString);
			setAccelXMax(false,v);
			return true;
		}else if(substr.indexOf("1,") != -1){
			String valString = inputString.substring(10);
			float v = convertStrToFloat(valString);
			setMaxT(false, v);
			return true;
		}else if(substr.indexOf("2,") != -1){
			String valString = inputString.substring(10);
			float v = convertStrToFloat(valString);
			setMinT(false,v);
			return true;
		}else if(substr.indexOf("3,") != -1){
			String valString = inputString.substring(10);
			float v = convertStrToFloat(valString);
			setDToverdtMax(false, (double)v);
			return true;
		}else if(substr.indexOf("50,") != -1){
			String valString = inputString.substring(11);
			float v = convertStrToFloat(valString);
			autoT_Trans_Sec = v;
			return true;
		}else if(substr.indexOf("51,") != -1){
			String valString = inputString.substring(11);
			float v = convertStrToFloat(valString);
			autoT_Degrees_Changed = v;
			return true;
		}else if(substr.equals("60,") != -1){
			//String response = cmd_RESPPARA+":60,"+CURRENT_TEMP_FOR_CALIBRATION+"\n";
			//sendResponse(response);
			return true;
		}else if(substr.equals("61,") != -1){
			//String response = cmd_RESPPARA+":61,"+CURRENT_ACC_X_FOR_CALIBRATION+"\n";
			//sendResponse(response);
			return true;
		}else if(substr.equals("62,") != -1){
			//String response = cmd_RESPPARA+":62,"+CURRENT_ACC_Y_FOR_CALIBRATION+"\n";
			//sendResponse(response);
			return true;
		}else if(substr.equals("63,") != -1){
			//String response = cmd_RESPPARA+":63,"+CURRENT_ACC_Z_FOR_CALIBRATION+"\n";
			//sendResponse(response);
			return true;
		}else{
			return false;
		}
	}else if((inputString.indexOf(cmd_ALARMX) != -1) ||
		(inputString.indexOf(cmd_ALARMY) != -1) ||
		(inputString.indexOf(cmd_ALARMZ) != -1)){

		int ledYellowAccel = 2;
		digitalWrite(ledYellowAccel, HIGH);
		return true;
		
	}else if(inputString.indexOf(cmd_ALARMTEMP) != -1){
		int ledYellowTemp= 5;
		digitalWrite(ledYellowTemp, HIGH);
		return true;
	}else if(inputString.indexOf(cmd_RESETMINMAX) != -1){

		setMinMaxValues();
		return true;
	}else{
		return false;
	}			
}

float ProtocolHandler::convertStrToFloat(String s){
	char buf[s.length()+1];
  	s.toCharArray(buf,s.length()+1);
  	return atof(buf);
}

void ProtocolHandler::sendResponse(String response)
{
	Serial.print(response);
	//char buffer[50];
	//response.toCharArray(buffer,50);
	//Serial.write(buffer);
}

float ProtocolHandler::getTemp(int AnalogInputNumber,int OutputUnit,float B,float T0,float R0,float R_Balance)
{
  float R,T;

  R=(1024.0f*R_Balance/float(analogRead(AnalogInputNumber)))-R_Balance;
  T=1.0f/(1.0f/T0+(1.0f/B)*log(R/R0));

  T-=273.15f;

  return T;
}

float ProtocolHandler::getTempWTable(int AnalogInputNumber, float R_Balance)
{
  long T;
  int R;
  
  const long T_table[]  = {-10, -5, 0, 5, 10, 15, 20, 25, 30, 35, 40, 45, 50};
  const long R_table[]  = {107900, 84250, 66240, 52450, 41810, 33550, 27080, 22000, 17970, 14770, 12200, 10120, 8447};

  R=(1024*R_Balance/analogRead(AnalogInputNumber))-R_Balance;
  
  int index_left;
  int index_right;
  long d_min = R_table[0] - R; 

  for( int i=0; i<13; i++){
    long diff = R_table[i] - R;
    if(diff>0){
      if(diff<d_min){
        d_min = diff;
        index_left = i;
      }
    }
  }
  index_right = index_left + 1;
  
  T= T_table[index_right]+(((T_table[index_left]-T_table[index_right])*(R-R_table[index_right]))/(R_table[index_left]-R_table[index_right]));

  return T;
}

String ProtocolHandler::ConvertFloatToString(float val, int val_size, int val_before_precision, int val_after_precision){
  if(val == 0){
  	return "0";
  }else{
  	char valStr[25];
  	return floatToString(valStr,  val,  val_before_precision, 0 , false);
  }
  //dtostrf(val,val_before_precision,val_after_precision,valStr);
  //return String(valStr);
}

double ProtocolHandler::voltage2g(double accAnalogValue){
  double voltage = (accAnalogValue) * arduino_power_supply / 1024;
  return (voltage - zero_g_bias) * 1000 / 330;
}



//************************************
// getters and setters for min & max
//************************************

void ProtocolHandler::setMinT(boolean b4reset, float val){
	if(b4reset == false){
		minT = val;
	}else{
		minT_b4_reset = val;
	}
}
float ProtocolHandler::getMinT(boolean b4reset){
	if(b4reset == false){
		return minT;
	}else{
		return minT_b4_reset;
	}
}

void ProtocolHandler::setMaxT(boolean b4reset, float val){
	if(b4reset == false){
		maxT = val;
	}else{
		maxT_b4_reset = val;
	}
}
float ProtocolHandler::getMaxT(boolean b4reset){
	if(b4reset == false){
		return maxT;
	}else{
		return maxT_b4_reset;
	}
}

void ProtocolHandler::setAccelXMin(boolean b4reset, double val){
	if(b4reset == false){
		accelXMin = val;
	}else{
		accelXMin_b4_reset = val;
	}
}
double ProtocolHandler::getAccelXMin(boolean b4reset){
	if(b4reset == false){
		return accelXMin;
	}else{
		return accelXMin_b4_reset;
	}
}

void ProtocolHandler::setAccelYMin(boolean b4reset, double val){
	if(b4reset == false){
		accelYMin = val;
	}else{
		accelYMin_b4_reset = val;
	}
}
double ProtocolHandler::getAccelYMin(boolean b4reset){
	if(b4reset == false){
		return accelYMin;
	}else{
		return accelYMin_b4_reset;
	}
}

void ProtocolHandler::setAccelZMin(boolean b4reset, double val){
	if(b4reset == false){
		accelZMin = val;
	}else{
		accelZMin_b4_reset = val;
	}
}
double ProtocolHandler::getAccelZMin(boolean b4reset){
	if(b4reset == false){
		return accelZMin;
	}else{
		return accelZMin_b4_reset;
	}
}

void ProtocolHandler::setAccelXMax(boolean b4reset, float val){
	if(b4reset == false){
		accelXMax = (double) val;
	}else{
		accelXMax_b4_reset = val;
	}
}
double ProtocolHandler::getAccelXMax(boolean b4reset){
	if(b4reset == false){
		return accelXMax;
	}else{
		return accelXMax_b4_reset;
	}
}

void ProtocolHandler::setAccelYMax(boolean b4reset, double val){
	if(b4reset == false){
		accelYMax = val;
	}else{
		accelYMax_b4_reset = val;
	}
}
double ProtocolHandler::getAccelYMax(boolean b4reset){
	if(b4reset == false){
		return accelYMax;
	}else{
		return accelYMax_b4_reset;
	}
}

void ProtocolHandler::setAccelZMax(boolean b4reset, double val){
	if(b4reset == false){
		accelZMax = val;
	}else{
		accelZMax_b4_reset = val;
	}
}
double ProtocolHandler::getAccelZMax(boolean b4reset){
	if(b4reset == false){
		return accelZMax;
	}else{
		return accelZMax_b4_reset;
	}
}

void ProtocolHandler::setDToverdtMax(boolean b4reset, double val){
	if(b4reset == false){
		dToverdtMax = val;
	}else{
		dToverdtMax_b4_reset = val;
	}
}
double ProtocolHandler::getDToverdfMax(boolean b4reset){
	if(b4reset == false){
		return dToverdtMax;
	}else{
		return dToverdtMax_b4_reset;
	}
}

void ProtocolHandler::setAutoT_Trans_Sec(boolean b4reset, double val){
	if(b4reset == false){
		autoT_Trans_Sec = val;
	}else{
		autoT_Trans_Sec = val;
	}
}
double ProtocolHandler::getAutoT_Trans_Sec(boolean b4reset){
	if(b4reset == false){
		return autoT_Trans_Sec;
	}else{
		return autoT_Trans_Sec;
	}
}
void ProtocolHandler::setAutoT_Degrees_Changed(boolean b4reset, double val){
	if(b4reset == false){
		autoT_Degrees_Changed = val;
	}else{
		autoT_Degrees_Changed = val;
	}
}
double ProtocolHandler::getAutoT_Degrees_Changed(boolean b4reset){
	if(b4reset == false){
		return autoT_Degrees_Changed;
	}else{
		return autoT_Degrees_Changed;
	}
}




void ProtocolHandler::setminT_b4_reset(float val){
	minT_b4_reset = val;
}
float ProtocolHandler::getminT_b4_reset(){
	return minT_b4_reset;
}

void ProtocolHandler::setmaxT_b4_reset(float val){
	maxT_b4_reset = val;
}
float ProtocolHandler::getmaxT_b4_reset(){
	return maxT_b4_reset;
}

void ProtocolHandler::setaccelXMin_b4_reset(float val){
	accelXMin_b4_reset = val;
}
float ProtocolHandler::getaccelXMin_b4_reset(){
	return accelXMin_b4_reset;
}

void ProtocolHandler::setaccelXMax_b4_reset(float val){
	accelXMax_b4_reset = val;
}
float ProtocolHandler::getaccelXMax_b4_reset(){
	return accelXMax_b4_reset;
}

void ProtocolHandler::setaccelYMin_b4_reset(float val){
	accelYMin_b4_reset = val;
}
float ProtocolHandler::getaccelYMin_b4_reset(){
	return accelYMin_b4_reset;
}

void ProtocolHandler::setaccelYMax_b4_reset(float val){
	accelYMax_b4_reset = val;
}
float ProtocolHandler::getaccelYMax_b4_reset(){
	return accelYMax_b4_reset;
}

void ProtocolHandler::setaccelZMin_b4_reset(float val){
	accelZMin_b4_reset = val;
}
float ProtocolHandler::getaccelZMin_b4_reset(){
	return accelZMin_b4_reset;
}

void ProtocolHandler::setaccelZMax_b4_reset(float val){
	accelZMax_b4_reset = val;
}
float ProtocolHandler::getaccelZMax_b4_reset(){
	return accelZMax_b4_reset;
}

void ProtocolHandler::setdToverdtMax_b4_reset(float val){
	dToverdtMax_b4_reset = val;
}
float ProtocolHandler::getdToverdtMax_b4_reset(){
	return dToverdtMax_b4_reset;
}






void ProtocolHandler::setMinMaxValues(){
  minT = -10.0;
  maxT = 30.0;
  accelXMin = -90.00;
  accelXMax = 90.00;
  accelYMin = -90.00;
  accelYMax = 90.00;
  accelZMin = -90.00;
  accelZMax = 90.00;  

  minT_b4_reset = 0;
  maxT_b4_reset = 0;
  accelXMin_b4_reset = 0;
  accelXMax_b4_reset = 0;
  accelYMin_b4_reset = 0;
  accelYMax_b4_reset = 0;
  accelZMin_b4_reset = 0;
  accelZMax_b4_reset = 0;
  dToverdtMax_b4_reset = 0;
}

char * ProtocolHandler::floatToString(char * outstr, float value, int places, int minwidth, bool rightjustify) {
    // this is used to write a float value to string, outstr.  oustr is also the return value.
    int digit;
    float tens = 0.1;
    int tenscount = 0;
    int i;
    float tempfloat = value;
    int c = 0;
    int charcount = 1;
    int extra = 0;
    // make sure we round properly. this could use pow from <math.h>, but doesn't seem worth the import
    // if this rounding step isn't here, the value  54.321 prints as 54.3209

    // calculate rounding term d:   0.5/pow(10,places)  
    float d = 0.5;
    if (value < 0)
        d *= -1.0;
    // divide by ten for each decimal place
    for (i = 0; i < places; i++)
        d/= 10.0;    
    // this small addition, combined with truncation will round our values properly 
    tempfloat +=  d;

    // first get value tens to be the large power of ten less than value    
    if (value < 0)
        tempfloat *= -1.0;
    while ((tens * 10.0) <= tempfloat) {
        tens *= 10.0;
        tenscount += 1;
    }

    if (tenscount > 0)
        charcount += tenscount;
    else
        charcount += 1;

    if (value < 0)
        charcount += 1;
    charcount += 1 + places;

    minwidth += 1; // both count the null final character
    if (minwidth > charcount){        
        extra = minwidth - charcount;
        charcount = minwidth;
    }

    if (extra > 0 and rightjustify) {
        for (int i = 0; i< extra; i++) {
            outstr[c++] = ' ';
        }
    }

    // write out the negative if needed
    if (value < 0)
        outstr[c++] = '-';

    if (tenscount == 0) 
        outstr[c++] = '0';

    for (i=0; i< tenscount; i++) {
        digit = (int) (tempfloat/tens);
        itoa(digit, &outstr[c++], 10);
        tempfloat = tempfloat - ((float)digit * tens);
        tens /= 10.0;
    }

    // if no places after decimal, stop now and return

    // otherwise, write the point and continue on
    if (places > 0)
    outstr[c++] = '.';


    // now write out each decimal place by shifting digits one by one into the ones place and writing the truncated value
    for (i = 0; i < places; i++) {
        tempfloat *= 10.0; 
        digit = (int) tempfloat;
        itoa(digit, &outstr[c++], 10);
        // once written, subtract off that digit
        tempfloat = tempfloat - (float) digit; 
    }
    if (extra > 0 and not rightjustify) {
        for (int i = 0; i< extra; i++) {
            outstr[c++] = ' ';
        }
    }


    outstr[c++] = '\0';
    return outstr;
}